/**
 * @file record_tf.cpp
 * @brief 记录SLAM的轨迹真值
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2022-01-19
 * 
 * @copyright Copyright (c) 2022  WHU-I2NAV
 * 
 */
#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <iomanip>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

const double R2D = 180.0 / 3.1415926535;
const double D2R = 3.1415926535 / 180.0;

/**
 * @brief 数据类型转换联合体，用于float和字节数组间的转换
 */
typedef union
{
	float data;
	unsigned char data8[4];
} data_u;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "record_tf");
	ros::NodeHandle nh;
	ros::NodeHandle private_nh("~");

	time_t tt = time(NULL);
    struct tm* t = localtime(&tt);

    std::stringstream filename;
    filename << "/home/nano/workspace/ai-robot/catkin_ws/" << "truth_" << t->tm_year + 1900 << "_"
                                                                    << t->tm_mon + 1 << "_" 
                                                                    << t->tm_mday << "_" 
                                                                    << t->tm_hour << "_" 
                                                                    << t->tm_min << "_" 
                                                                    << t->tm_sec << ".txt";
    std::ofstream output_file_s;
    output_file_s.open(filename.str().c_str());
    output_file_s.precision(15);

    auto startTime = ros::Time::now();

	ros::Rate loop_rate(20); 

	    // 监听tf
    tf::TransformListener listener;
    tf::StampedTransform transform;

    while(ros::ok())
	{
		try
		{
			// 监听从base_link到map的转换
			listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
		}
		catch(tf::TransformException &ex)
		{
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		auto currentTime = ros::Time::now();
		geometry_msgs::Pose  pose;
		pose.position.x = transform.getOrigin().x();
		pose.position.y = transform.getOrigin().y();
		pose.orientation.x = transform.getRotation().x();
		pose.orientation.y = transform.getRotation().y();
		pose.orientation.z = transform.getRotation().z();
		pose.orientation.w = transform.getRotation().w();

		output_file_s   << (currentTime - startTime).toSec() << " "
						<< pose.position.x << " " << pose.position.y << " "
						<< tf::getYaw(pose.orientation) << std::endl;

		ROS_INFO("pose: %f %f %f", pose.position.x, pose.position.y, tf::getYaw(pose.orientation));

		loop_rate.sleep();
	}

	return 0; 
}